#include <ros/ros.h>
#include <geometry_msgs/Twist.h>  // for velocity commands
#include <geometry_msgs/TwistStamped.h>
#include <gazebo_msgs/ModelStates.h>

geometry_msgs::Pose pose;
geometry_msgs::Point position;
geometry_msgs::Quaternion orientation;
geometry_msgs::Twist twist;
geometry_msgs::Vector3 linear;
geometry_msgs::Vector3 angular;

geometry_msgs::Twist twist_show;

geometry_msgs::Point position_goal;
geometry_msgs::Quaternion orientation_goal;
double velocity = 0.5;

void msCallback(const gazebo_msgs::ModelStatesConstPtr& msg)
{
    int index = -1;
    std::vector<std::string> model_names = msg->name;
    for(size_t i=0;i < model_names.size();i++)
    {
        if(model_names[i]=="robot_1")
        {
            index = i;
        }
    }
    pose = msg->pose[index];
    twist_show = msg->twist[index];

    position = pose.position;
    orientation = pose.orientation;
    //linear = twist.linear;
    //angular = twist.angular;

    if (position.x <= 1 && position.y <= 1 && orientation.z-orientation.w < 0.01)
    {
        linear.x = linear.y = linear.z = 0;
        angular.x = angular.y = 0;
        angular.z =  velocity;
    }
    else if(position.y < position_goal.y-1)
    {
        linear.x = velocity;
        linear.y = linear.z = 0;
        angular.x = angular.y = angular.z = 0;

    }
    else if(position.x - position_goal.x >0.05)
    {
        linear.x = velocity;
        linear.y = linear.z = 0;
        angular.x = angular.y = 0;
        if(orientation.w - orientation_goal.w < 0.01)
            angular.z = 0;
        else
            angular.z = velocity;

    }
    else
    {
        linear.x = linear.y = linear.z = 0;
        angular.x = angular.y = angular.z = 0;
    }

    //tan = position.y / position.x;

    //angular.z = 0.1*(orientation_goal.z - orientation.z);

    //std::cout<< std::endl << pose;
//    std::cout<< std::endl << position << std::endl << twist_show ;

}


int main(int argc, char** argv)
{
    //give the goal pose.
    position_goal.x = -7;
    position_goal.y = 6;
    position_goal.z = 0;
    orientation_goal.x = 0;
    orientation_goal.y = 0;
    orientation_goal.z = 1;
    orientation_goal.w = 0;


    ros::init(argc, argv, "move_to_dst");
    ros::NodeHandle nh("~");
    ros::NodeHandle n;


    ros::Publisher velocity_publisher_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
    ros::Subscriber robot_pose_subscriber = n.subscribe<gazebo_msgs::ModelStates>("/gazebo/model_states",1000,msCallback);
    ros::Rate rate(1000);

    while(ros::ok())
    {
        //use the callback function,which gets the pose information of robot and calculates the appropriate twist.
        ros::spinOnce();

        //form a twist and publish it to topic "cmd_vel".
        twist.linear = linear;
        twist.angular = angular;

        velocity_publisher_.publish(twist);
        rate.sleep();

    }
    return 0;

}